#ifndef _POINTCLOUDGENERATOR_H_
#define _POINTCLOUDGENERATOR_H_

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <opencv2/opencv.hpp>
#include <fstream>
#include <exception>
#include <chrono>
#include <pcl/filters/conditional_removal.h>
#include <cmath>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <opencv2/imgproc.hpp> 
#include <Eigen/Geometry>
#include <pcl/common/transforms.h>
#include <boost/shared_ptr.hpp>
class PointCloudGenerator {
public:
    PointCloudGenerator(const std::string& calib_file = std::string());

    bool loadCameraParams(const std::string& file_path);

    void generate(const cv::Mat &depth_16u);

    cv::Mat createPixelGrid();

    void precomputeUndistortionMap();

    void createCloud(const cv::Mat& depth, const cv::Mat& undistorted, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);

    void precomputeDenominator();

    void setparams(float tx, float ty, float tz, float yaw, float pitch, float roll, 
        float min_x, float max_x, float min_y, float max_y, float min_z, float max_z);

    cv::Mat mtx_, dist_;
    std::string calib_file_;
    int width_, height_;
    bool debug = false;
    cv::Mat undistorted_grid_; 
    std::vector<float> denominator_; // 预计算的分母值
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsample;
    pcl::PointCloud<pcl::PointXYZ>::Ptr remove_nan_point;
    pcl::PointCloud<pcl::PointXYZ>::Ptr final_filtered_cloud_;


    //中间图像区域
    int middle_height_start_;
    int middle_height_end_;
    int middle_height_;

    int middle_width_start_;
    int middle_width_end_;
    int middle_width_;

    boost::shared_ptr<pcl::VoxelGrid<pcl::PointXYZ>> voxelgrid;
    pcl::ApproximateVoxelGrid<pcl::PointXYZ>::Ptr approx_voxelgrid;

    double downsample_resolution;

    int filter_kernel_size;

    bool filter;
    bool warning;

    int warning_point_unm;

    float tx_, ty_, tz_, yaw_, pitch_, roll_;
    Eigen::Affine3f transform;

    float min_x_, max_x_, min_y_, max_y_, min_z_, max_z_;

};

#endif